import numpy as np


class PIDController:
    def __init__(self, Kp, Ki, Kd, integrator_max=None, integrator_min=None,max=None,min=None):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.integrator = 0
        self.prev_error = 0
        self.integrator_max = integrator_max
        self.integrator_min = integrator_min
        self.max = max
        self.min = min

    def reset(self):
        self.integrator = 0
        self.prev_error = 0

    def update(self, setpoint, measurement, dt):
        error = setpoint - measurement
        self.integrator += error * dt
        if self.integrator_max is not None and self.integrator > self.integrator_max:
            self.integrator = self.integrator_max
        if self.integrator_min is not None and self.integrator < self.integrator_min:
            self.integrator = self.integrator_min

        derivative = (error - self.prev_error) / dt
        output = self.Kp * error + self.Ki * self.integrator + self.Kd * derivative
        self.prev_error = error
        if self.max is not None and self.min is not None:
            output = np.clip(output,self.min,self.max)
        return output

class CascadePIDController:
    def __init__(self, outer_controller, inner_controller):
        self.outer_controller = outer_controller
        self.inner_controller = inner_controller

    def update(self, outer_setpoint, outer_measurement, inner_measurement, dt):
        outer_output = self.outer_controller.update(outer_setpoint, outer_measurement, dt)
        inner_output = self.inner_controller.update(outer_output, inner_measurement, dt)
        return inner_output


# import numpy as np
#
# class PIDController:
#     def __init__(self, Kp, Ki, Kd, integrator_max=None, integrator_min=None, max=None, min=None,dt=None):
#         self.Kp = Kp
#         self.Ki = Ki
#         self.Kd = Kd
#         self.integrator = 0
#         self.prev_error = 0
#         self.prev_prev_error = 0
#         self.integrator_max = integrator_max
#         self.integrator_min = integrator_min
#         self.max = max
#         self.min = min
#
#     def reset(self):
#         self.integrator = 0
#         self.prev_error = 0
#         self.prev_prev_error = 0
#
#     def update(self, setpoint, measurement):
#         error = setpoint - measurement
#         delta_error = error - self.prev_error
#         delta_prev_error = self.prev_error - self.prev_prev_error
#
#         # 更新积分项并应用饱和限制
#         # self.integrator += error
#         # if self.integrator_max is not None and self.integrator > self.integrator_max:
#         #     self.integrator = self.integrator_max
#         # if self.integrator_min is not None and self.integrator < self.integrator_min:
#         #     self.integrator = self.integrator_min
#
#         # 增量式PID公式
#         delta_output = self.Kp * delta_error \
#                        + self.Ki * error \
#                        + self.Kd * (delta_error - delta_prev_error)
#
#         # 更新历史误差
#         self.prev_prev_error = self.prev_error
#         self.prev_error = error
#
#         # 应用输出限幅
#         if self.max is not None and self.min is not None:
#             delta_output = np.clip(delta_output, self.min, self.max)
#
#         return delta_output
#
# class CascadePIDController:
#     def __init__(self, outer_controller, inner_controller,dt=None):
#         self.outer_controller = outer_controller
#         self.inner_controller = inner_controller
#
#     def update(self, outer_setpoint, outer_measurement, inner_measurement,dt=None):
#         outer_delta_output = self.outer_controller.update(outer_setpoint, outer_measurement)
#         inner_delta_output = self.inner_controller.update(outer_delta_output, inner_measurement)
#         return inner_delta_output
